/*
 * Position2d.java
 *
 * Created on 15 de noviembre de 2007, 04:19 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Robot;

import Robot.Sensors.Pose2DSensor;
import Robot.Sensors.Sensor;
import Util.*;

import java.util.Vector;
import java.awt.Color;
/**
 *
 * @author nxbot
 */
public abstract class Position2d extends Sensor implements Pose2DSensor {

    boolean ENABLE_NOISE=false;


    static final float RANDOM_LINEAR_VEL = 0.3f;
    static final float CORR_DESP= 0.5f;
    
    //EStos parametros deberian ser static para clases que heredan de esta.
    //Por ejemplo, hacer una clase que sea exclusiva para el nxbot, que ya tenga
    //estos parametros por defecto, a partir de las configuraciones realizadas
    float max_angle = (float) Math.PI/10;
    float corr_angle = (float) Math.PI/8;
    protected float maxSpeed = 0.7f;

    private float maxRotVel = 0.3f;
    private float minRotVel = 0.3f;

    protected Pose2d data;
    
    
    /** Creates a new instance of Position2d */
    public Position2d(Robot robot, String name, int index) {
        //constructor of father-class
        super(robot, name, index);
    }
    
    
    public void setNoise(boolean enable){
        ENABLE_NOISE=enable;
    }
    
    public void setMaxAngle(float angle){
        this.max_angle=angle;
    }
    public boolean getNoise(){
        return ENABLE_NOISE;
    
    }
     /**
     *
     * Leer data
     */
    public Pose2d read(int mode) {

        //if READ_MODE_WAIT it will waits until new data is available
        //else, returns the old data
        switch(mode)
        {
            case READ_MODE_WAIT:
                while(!update());
            break;
            case READ_MODE_NO_WAIT:
                update();
            break;
        
        }
        return data;   

    }
    
    public Vector2d getPositionVector(){
        return new Vector2d(data.getPx(),data.getPy(),Vector2d.MODE_XY);
    
    }

    //FIXME: esto debería recibir una pose para poder realizar control de orientacion
    //public boolean goToDir(Pose2d pose)
    //{ Vector2d posVector = new Vector2d(pose.getX(), pose.getY());


    public boolean goToDir(float magnitude,float angle){


        //random noise for the linear vel when rotating
        float noiseMag;
        int noiseSign;
        float corr;
        
        if(ENABLE_NOISE){
            noiseMag = (float)(Math.random()*RANDOM_LINEAR_VEL);
            noiseSign = 1;
            
        }else
        {
            noiseMag = 1;
            
            noiseSign = 1;
        }
        
        //first check if the robot really has to move
        if(magnitude!=0)
        {
            //First, magnitude should not be bigger than 1
            if(magnitude>1)
                    magnitude=1;

            //update position values
            read(READ_MODE_NO_WAIT);
            
            //calculate the headingDifference between the robots heading and the desired heading
            float headingDiff = angle - data.getPa();
            //Correction of angle, choose the shortest spin
            headingDiff = Vector2d.angleCorrMPI2PI(headingDiff);
            
//            System.out.println("POSITION2D: Heading calculation");
//            System.out.println("Heading of robot:" + Math.toDegrees(data.getPa()));
//            System.out.println("Desired heading:"+Math.toDegrees(angle));
//            System.out.println("Heading difference:"+Math.toDegrees(headingDiff)+"\n");
            
            //if the difference is bigger than the allowed then correct heading
            if(Math.abs(headingDiff)>max_angle){
               
                //an aid to linear velocity. If the angle is very thin, then 
                //add some linear velocity for soft drive.
                if(Math.abs(headingDiff) > corr_angle)
                    corr = 0;
                else
                    //this is the factor of linear velocity that will be added.
                    //it should not be bigger than 0.5. If the difference between
                    corr = 0.5f * (float)((Math.abs(headingDiff)-max_angle) / (corr_angle-max_angle));

                
                float corrSpeed = Math.abs(corr*noiseMag*noiseSign*magnitude);

                //Control proporcional de orientacion, donde el error es headingDiff
                //y la constante de proporcionalidad es minRotVel/max_angle


                float rotVel = (float) (minRotVel*Math.abs(headingDiff)/max_angle);

                if (rotVel >maxRotVel)
                    rotVel = maxRotVel;

                if(headingDiff<0)
                    setSpeed(corrSpeed,-rotVel);
                else
                    setSpeed(corrSpeed,rotVel);
                
                return false;

            }
            //the heading is right, then go forward
           else{
                
                //correction factor for angular velocity
                corr = headingDiff / max_angle;
                corr *= CORR_DESP;

                setSpeed(magnitude*maxSpeed,corr*magnitude);
               
           }
            return false; 
        }
        else{
            setSpeed(0,0);
            
        }
        return false;
    }
    
    
    
    
    
    
    
    
    
    
    public void setMaxSpeed(float maxSpeed)
    {
        this.maxSpeed = maxSpeed;
    }
    
    public void setRotControl(float minRotVel, float maxRotVel)
    {
        this.maxRotVel = maxRotVel;
        this.minRotVel = minRotVel;

    }
    
    
    
    public abstract void setSpeed(float val1, float val2);
    public abstract boolean update();
    public abstract boolean isReady();
    public abstract void setInitialPosition(Pose2d initial);
    
    
}
